/**
  ******************************************************************************
  * @file   	butterworth_filter.c
  * @author 	Peter chen
  * @version	
  * @date
  * @brief
  * @attention
  ******************************************************************************
  * @history
  * Ver	Date	Modified By	Why & What is modified
  *
  *
  *
  *
  * <h2><center>&copy; COPYRIGHT 2014 INMOTION TECHNOLOGIES CO.,LTD.</center></h2>
  ******************************************************************************
  */
#include "butterworth_filter.h"

/** @addtogroup 
  * @{
  */

/** @addtogroup 
  * @{
  */ 
  
/** @defgroup 
  * @{
  */

/** @defgroup _Private_TypesDefinitions
  * @{
  */

/**
  * @}
  */

/** @defgroup _Private_Defines
  * @{
  */  
  
/**
  * @}
  */

/** @defgroup _Private_Macros
  * @{
  */

/**
  * @}
  */

/** @defgroup _Private_Variables
  * @{
  */
// static float factorB[4] = {0.038, 0.115, 0.115, 0.038};
// static float factorA[4] = {1.000, -1.339, 0.817, -0.172};
// static u32 prevX[3] = {0};
// static u32 prevY[3] = {0};
static float factorB[2] = {15.865, 15.865};
static float factorA[2] = {1.000, -968.27};
static float prevX = 0;
static float prevY = 0;
/**
  * @}
  */

/** @defgroup _Private_FunctionPrototypes
  * @{
  */
 
/**
  * @}
  */

/** @defgroup _Private_Functions
  * @{
  */ 
  
/**
  * @}
  */
/**
  * @brief
  * @param  CANx:
  * @note   
  * @retval Configure result:
  *		- 0	fail
  *		- 1	success
  */
void BW_Filter_Init(u32 invar)
{
// 	u8 i = 0;
// 	for(i = 0; i < 1; i++){
// 		prevX[i] = prevY[i] = invar;
// 	}
	prevX = prevY = (float)invar;
}
/**
  * @brief
  * @param  invar:
  * @note   
  * @retval outvar
  */
u32 BW_Filter_int(u32 invar)
{
// 	u8 i = 0;
	float outvar = 0.0;
	
// 	for(i = 1; i < 2; i++){
		outvar = outvar + factorB[1] * prevX - factorA[1] * prevY;
// 	}
	outvar += factorB[0] * invar;
	outvar /= 1000.0;
	
// 	for(i = 1; i > 0; i--){
// 		prevX[i] = prevX[i - 1];
// 		prevY[i] = prevY[i - 1];
// 	}
	prevX = (float)invar;
	prevY = (float)outvar;
	
	return (u32)outvar;
}
/**
  * @}
  */
  
/**
  * @}
  */  
  
/**
  * @}
  */

/********* (C) COPYRIGHT 2014 INMOTION TECHNOLOGIES CO.,LTD. ***END OF FILE***/
